//
// Created by HP on 2024/2/18.
//

#include "FOC_thread.h"
#include "aurora_foc.h"
#include "aurora_motor.h"
ALIGN(RT_ALIGN_SIZE)
char foc_thread_stack[FOC_THREAD_STACK_SIZE];
struct rt_thread foc_thread;
uint32_t IL_value[2];
uint32_t IR_value[2];


uint8_t FOCL_TIME_FLAG = DISABLE;
uint8_t FOCR_TIME_FLAG = DISABLE;

Aurora_Motor motor;
AS5047_Driver as5047L(&hspi3,SPI3_CS_GPIO_Port,SPI3_CS_Pin);
AS5047_Driver as5047R(&hspi2,SPI2_CS_GPIO_Port,SPI2_CS_Pin);


void FOC_thread(void *param){
    motor.add_AS5047_Driver(&as5047L,&as5047R);
    motor.enable_current_sampling(ENABLE,ENABLE);

    motor.start_pwm();

    while( true ){
        if(ui2foc_msg.cmd != IS_NONE){
            ui2foc_msg.cmd = IS_NONE;
            motor.dealmsgEvent_from_UI(&ui2foc_msg);
        }
        if(comm2foc_msg.cmd != IS_NONE){
            comm2foc_msg.cmd = IS_NONE;
            motor.dealmsgEvent_from_COMM(&comm2foc_msg);
        }

//        if( FOCL_TIME_FLAG == ENABLE){
//            motor.main_deal(FOCL_TIME_FLAG,DISABLE,IL_value[0],IL_value[1],IR_value[0],IR_value[1]);
//            FOCL_TIME_FLAG = DISABLE;
//        }
//        if( FOCR_TIME_FLAG == ENABLE){
//            motor.main_deal(DISABLE,FOCR_TIME_FLAG,IL_value[0],IL_value[1],IR_value[0],IR_value[1]);
//            FOCR_TIME_FLAG = DISABLE;
//        }
       // usb_printf("%d  %d   %d   %d \r\n",IL_value[0],IL_value[1],IR_value[0],IR_value[1]);

      //  rt_thread_suspend(&foc_thread); //执行完foc任务后，挂起
      //  rt_schedule();                         //即可进行上下文切换
        usb_printf("%ld,%ld,%ld,%ld\r\n",IL_value[0],IL_value[1],IR_value[0],IR_value[1]);
        rt_thread_mdelay(5);
    }

}
/** 定时器1/8 中断函数 **/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
 //   rt_interrupt_enter();
//    if( htim == &htim1){
//        FOCL_TIME_FLAG = ENABLE;
//        rt_thread_resume(&foc_thread);  //恢复FOC线程
//    }
//    else if(htim == &htim8){
//        FOCR_TIME_FLAG = ENABLE;
//        rt_thread_resume(&foc_thread);  //恢复FOC线程
//    }
    if (htim->Instance == TIM4) {
        HAL_IncTick();
    }
 //   rt_interrupt_leave();
}

/** adc 注入转换完成中断 **/
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc){
    rt_interrupt_enter();
    if( hadc == &hadc1){
        IL_value[0] = hadc1.Instance->JDR1;// 获取转换结果
        IL_value[1] = hadc1.Instance->JDR2; // 获取转换结果
       // FOCL_TIME_FLAG = ENABLE;
      //  rt_thread_resume(&foc_thread);  //恢复FOC线程
      //  usb_printf("%ld,%ld,\r\n",IL_value[0],IL_value[1]);
//        motor.main_deal(FOCL_TIME_FLAG,DISABLE,IL_value[0],IL_value[1],IR_value[0],IR_value[1]);
    }
    else if( hadc == &hadc2){
        IR_value[0] = hadc2.Instance->JDR1;// 获取转换结果
        IR_value[1] = hadc2.Instance->JDR2; // 获取转换结果
      //  FOCR_TIME_FLAG = ENABLE;
      //  rt_thread_resume(&foc_thread);  //恢复FOC线程
        motor.main_deal(ENABLE,ENABLE,IL_value[0],IL_value[1],IR_value[0],IR_value[1]);

    }
    rt_interrupt_leave();
}